/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
* This module filters a pose distribution based on 
* - Odometry readings
* - Pose measurements
* using particle filter
*/
#include "CParticleFilter.h"

#ifndef _POSE_MCL_H_
#define _POSE_MCL_H_

class CPoseMCL{
	public:
		CPoseMCL();
		
		/**
		* Initializes internal variables
		* @param pDistTh The largest acceptaple distance error(m) (more than this will give zero likelihood)
		* @param pDist2thLik If the likelihood is more than threshold the likelihood is cutted to this value
		* @param pHeadErrorTh The largest acceptaple heading error (rad)
		* @param pHeadCutTh If the likelihood is more than threshold the likelihood is cutted to this value
		* 
		* Below is an octave script for testing the parameters
			pDistTh = 2;
			pDist2thLik=0.995;
			dist = 0:0.01:pDistTh;
			dist2=dist.^2;
			cost=(pDistTh*pDistTh-dist2)/(pDistTh*pDistTh);
			cost(cost>pDist2thLik)=pDist2thLik;
			plot(dist,cost,'.');
			grid on;
			
			pHeadErrorTh = 45*pi/180;
			pHeadCutTh = 0.95;
			angle = 0:0.01:pHeadErrorTh;
			cost = (pHeadErrorTh-angle)/pHeadErrorTh;
			cost(cost>pHeadCutTh)=pHeadCutTh;
			plot(angle*180/pi,cost);
		*/
		CPoseMCL(float pDistTh, float pDist2thLik, float pHeadErrorTh, float pHeadCutTh );
		/**
		* Calculates the likelihood of particle distribution
		* @param P pose measurement for the current distribution
		**/
		void calculateLikelihood(mcl::pose P);
		
		void predict(mcl::pose dP,mcl::pose variance);
		void update(mcl::pose P);
		
		CParticleFilter filt; ///<the particle filter used
		
		/**
		* Initializes the filter - uses normal random method for calculating the initial distribution
		* Loads the map from line file, reserves memory for particles, sets the initial distribution
		* @p num_of_particles The number of particles in the filter
		* @p init_pose the initial pose
		* @p initVariance the variance for the initial quess
		*/
		void initialize(int num_of_particles, mcl::pose initPose,	mcl::pose initVariance);
		
		/**
		* Check if the distribution is valid for given pose
		*/
		bool isValidDistribution(mcl::pose P);	
	private:
		
		void initializeVariables(float pDistTh, float pDist2thLik, float pHeadErrorTh, float pHeadCutTh );
		
		float paramDist2th;
		float paramDist2thLik;
		float paramHeadErrorTh;
		float paramHeadCutTh;
		
};


#endif

